import cv2
import math
import numpy as np
import random

class approx:
    def __init__(self):
        self.approxs=None #图上的所有顶点
        self.returnlist = None# 返回拆分后连通性列表,
        self.concave_list = None # 凹点，要进行凸剖分的
        self.point_map = None #一个连通域的所有顶点，有-1的
        self.linelist =None #地图上所有的拟合线，用起点终点表示的
        self.vplist=None #一个点的可见点
        self.p_min=None #最近的可视点
        self.cutlinelist=[] #凸剖分的切割线,编号[点，点]
        self.point_list=None # 一个点在地图上的位置，编号，（s,n(编号),e,1）
        self.cutline = None  # 一次凸剖分的切割线
        self.newconcave=None # 凸剖分切割后的新凸点
        self.bridgelist=None # bridgelist[切割线]=[切割线的一个岛1，切割线的一个岛2]
        self.islandlist = [] # 岛群，一个岛代表一个凸多边形，由点和凸剖分的切割线组成
        self.cutline_count = 0 #
        self.pointcutlist=None #编号，[连接点编号，切割线编号]
        self.IMGmap=None



    def get_approx_list(self, hchy):
        '''
        返回多个连通区域
        一般只有一个，有多个细小的调 ratio
        '''
        self.returnlist = []
        for i in range(0, len(hchy)):
            hchylayer = 0
            hchyp = i
            while hchy[hchyp][3] != -1:  # 有下一级轮廓
                hchyp = hchy[hchyp][3]  # 下一级标号
                hchylayer += 1  # 算层数
            # print(hchylayer)
            if (hchylayer % 2 == 0):
                temp_list = self.approxs[i].tolist()  # 打包成一个
                hchyp = hchy[i][2]  # 下一级
                if hchyp != -1:  # 如果有下一级（一定是奇数级），不是最基础的轮廓,层数不为0
                    temp_list.append([-1, -1])
                    temp_list += self.approxs[hchyp].tolist()  # 这里加奇数级
                    while hchy[hchyp][0] != -1:
                        hchyp = hchy[hchyp][0]
                        temp_list.append([-1, -1])
                        temp_list += self.approxs[hchyp].tolist()
                self.returnlist.append(temp_list)  # 层数是0直接添加

        # temp = np.ones((1463, 1218,3), np.uint8) * 255
        # cv2.drawContours(temp, approxs, -1, (0, 255, 0), 1)
        # for i in range(0, len(returnlist)):
        #     color = (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255))
        #     #print(returnlist[i])
        #     for point in returnlist[i] :
        #          print(point)
        #          if point[0]!=-1:
        #             cv2.circle(temp, point, 5, color, 2)
        #          else:
        #             color = (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255))
        # cv2.imshow('approxs', temp)
        # cv2.waitKey()

        return self.returnlist  # 返回拆分后连通性列表

    def get_approx(self,img, robotpixel):
        # 提取轮廓
        contour, hierarchy = cv2.findContours(img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        # 提取轮廓
        '''
        findContours函数接收三个参数，分别是图像，轮廓检索模式，以及轮廓近似方法。

        opencv2返回两个值：一个是轮廓本身contours，还有一个是每条轮廓对应的属性hierarchy。
        contours：检测到的轮廓，以numpy数组的形式返回。
        hierarchy：轮廓的层次结构，以numpy数组的形式返回。
        offset：可选参数，表示轮廓中每个点的偏移量，通常设置为(0,0)。
        '''
        # print(len(contour))
        # print(hierarchy)
        self.approxs = [[]] * len(contour)
        hchy = [[]] * len(contour)
        epsilon = int(robotpixel / 3)
        # print(hierarchy)
        # print(img.shape)
        for i in range(0, len(contour)):
            approx = cv2.approxPolyDP(contour[i], epsilon, True)  # 将每个轮廓近似为一个多边形
            self.approxs[i] = approx[:, 0, :][::-1]
            hchy[i] = hierarchy[0][i]
            # print(hchy[i])

        # temp = np.ones(img.shape, np.uint8) * 255
        # # temp2 = np.ones(img.shape, np.uint8) * 255
        # # 画轮廓：temp为白色幕布，contours为轮廓， -1表示画所有轮廓，颜色：绿色，厚度
        # cv2.drawContours(temp, approxs, -1, (0, 255, 0), 3)
        # '''
        # 把第三个参数改一下可以看approxs里的每一个轮廓
        # image：需要绘制轮廓的图像，可以是原始图像或者是黑白图像。
        # contours：要绘制的轮廓。需要传入一个列表，列表中的每一个元素是一个轮廓，每个轮廓由若干个点坐标组成。
        # contourIdx：需要绘制的轮廓在列表中的索引，如果传入 -1，表示绘制所有的轮廓。
        # color：轮廓的颜色。可以是一个颜色值，如 (0, 255, 0)，表示绿色；也可以是一个 BGR 三通道的数组，如 [255, 0, 0]，表示蓝色。
        # thickness：轮廓线的粗细。默认值为 1，如果需要填充轮廓，则可以设置为 -1。
        # '''
        # #cv2.drawContours(temp2, contour,-1, (0, 255, 0), 3)
        # cv2.imshow('approxs', temp)
        # #cv2.imshow('contour', temp2)
        # cv2.waitKey()

        return self.get_approx_list( hchy)

    def approx_work(self,IMGmap, ratio, robotsize, thresh):
        # 路径二值化
        self.img_gray = cv2.cvtColor(IMGmap, cv2.COLOR_BGR2GRAY)
        ret, self.img_bin = cv2.threshold(self.img_gray, thresh, 255, cv2.THRESH_BINARY)

        # 二值化后的图像
        # cv2.imshow("1",img_bin)
        # cv2.waitKey(0)
        # 二值化
        robotpixel = int(robotsize / ratio)
        erode_r = int(robotpixel / 2)
        element = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (erode_r, erode_r))
        # 构造核
        erosion = cv2.erode(self.img_bin, element, iterations=1)  # 腐蚀
        # erosion是腐蚀后的图像
        # cv2.imshow("1",erosion)
        # cv2.waitKey(0)
        return [self.get_approx(erosion, robotpixel), list(self.img_bin.shape)]

    def find_concave(self,point_map):
        s_point_temp = 0
        n_point_temp = 0
        point_list_len = 0
        self.point_list = [self.VP([0,0,0,0]) for i in range(len(point_map))]
        self.linelist = []
        linelistlen = 0
        self.concave_list = []
        concave_listlen = 0
        self.point_map=point_map

        while True:
            point_list_len += 1#起点是最后处理的，最开始处理的是二号点

            while True:#遍历点，快结束了为真
                if n_point_temp + 2 >= len(self.point_map) or self.point_map[n_point_temp + 2][0] < 0:
                    # 遍历到倒数第二个点，实际上在判断最后一个点
                    self.linelist.append(self.Line([n_point_temp, n_point_temp + 1]))
                    v1 = [self.point_map[n_point_temp + 1][0] - self.point_map[n_point_temp][0],
                          self.point_map[n_point_temp + 1][1] - self.point_map[n_point_temp][1]]
                    v2 = [self.point_map[s_point_temp][0] - self.point_map[n_point_temp + 1][0],
                          self.point_map[s_point_temp][1] - self.point_map[n_point_temp + 1][1]]
                    r = np.cross(v1, v2)
                    # v1_x = self.point_map[n_point_temp + 1][0] - self.point_map[n_point_temp][0]
                    # v1_y = self.point_map[n_point_temp + 1][1] - self.point_map[n_point_temp][1]
                    # v2_x = self.point_map[s_point_temp][0] - self.point_map[n_point_temp + 1][0]
                    # v2_y = self.point_map[s_point_temp][1] - self.point_map[n_point_temp + 1][1]
                    # r = v1_x * v2_y - v1_y * v2_x

                    if r > 0:
                        self.point_list[point_list_len]=self.VP([n_point_temp, n_point_temp + 1, s_point_temp, 1])
                        point_list_len += 1
                    else:
                        self.point_list[point_list_len]=self.VP([n_point_temp, n_point_temp + 1, s_point_temp, -1])
                        point_list_len += 1
                        # self.concave_list[concave_listlen]=self.VP([n_point_temp, n_point_temp + 1, s_point_temp, -1])
                        self.concave_list.append(self.VP([n_point_temp, n_point_temp + 1, s_point_temp, -1]))
                        concave_listlen += 1

                    # 遍历到倒数第一个点，实际上在判断起点
                    n_point_temp += 1
                    self.linelist.append(self.Line([n_point_temp, s_point_temp]))
                    v1 = [self.point_map[s_point_temp][0] - self.point_map[n_point_temp][0],
                          self.point_map[s_point_temp][1] - self.point_map[n_point_temp][1]]
                    v2 = [self.point_map[s_point_temp + 1][0] - self.point_map[s_point_temp][0],
                          self.point_map[s_point_temp + 1][1] - self.point_map[s_point_temp][1]]
                    r = np.cross(v1, v2)
                    # v1_x = self.point_map[s_point_temp][0] - self.point_map[n_point_temp][0]
                    # v1_y = self.point_map[s_point_temp][1] - self.point_map[n_point_temp][1]
                    # v2_x = self.point_map[s_point_temp + 1][0] - self.point_map[s_point_temp][0]
                    # v2_y = self.point_map[s_point_temp + 1][1] - self.point_map[s_point_temp][1]
                    # r = v1_x * v2_y - v1_y * v2_x

                    if r > 0:
                        self.point_list[s_point_temp]=self.VP([n_point_temp, s_point_temp, s_point_temp + 1, 1])
                    else:
                        self.point_list[s_point_temp]=self.VP([n_point_temp, s_point_temp, s_point_temp + 1, -1])
                        self.concave_list.append(self.VP([n_point_temp, s_point_temp, s_point_temp + 1, -1]))
                        concave_listlen += 1

                    point_list_len += 1
                    break

                else:#这才是大部分情况
                    self.linelist.append(self.Line([n_point_temp, n_point_temp + 1]))
                    v1 = [self.point_map[n_point_temp + 1][0] - self.point_map[n_point_temp][0],
                          self.point_map[n_point_temp + 1][1] - self.point_map[n_point_temp][1]]
                    v2 = [self.point_map[n_point_temp + 2][0] - self.point_map[n_point_temp + 1][0],
                          self.point_map[n_point_temp + 2][1] - self.point_map[n_point_temp + 1][1]]
                    r = np.cross(v1, v2)
                    # v1_x = self.point_map[n_point_temp + 1][0] - self.point_map[n_point_temp][0]
                    # v1_y = self.point_map[n_point_temp + 1][1] - self.point_map[n_point_temp][1]
                    # v2_x = self.point_map[n_point_temp + 2][0] - self.point_map[n_point_temp + 1][0]
                    # v2_y = self.point_map[n_point_temp + 2][1] - self.point_map[n_point_temp + 1][1]
                    # r = v1_x * v2_y - v1_y * v2_x

                    if r > 0:
                        self.point_list[point_list_len]=self.VP([n_point_temp, n_point_temp + 1, n_point_temp + 2, 1])
                        point_list_len += 1
                    else:
                        self.point_list[point_list_len]=self.VP([n_point_temp, n_point_temp + 1, n_point_temp + 2, -1])
                        point_list_len += 1
                        # self.concave_list[concave_listlen]=self.VP([n_point_temp, n_point_temp + 1, n_point_temp + 2, -1])
                        self.concave_list.append( self.VP([n_point_temp, n_point_temp + 1, n_point_temp + 2, -1]))
                        concave_listlen += 1

                n_point_temp += 1
            n_point_temp += 1
            if n_point_temp >= len(self.point_map):
                break
            else:
                n_point_temp += 1
                s_point_temp = n_point_temp

        # 返回计算结果
        return self.point_list, self.linelist, self.concave_list

    def Startcut(self):
        # 单点个凸剖分
        # print(self.concave_list[9])

        # 向看单个剖分用这部分，下面的全注释掉
        # vp = self.concave_list[30]
        # self.ViewablePoint(vp)
        # self.WeightCut(vp)

        # print(self.point_map[vp[0]])
        # print(len(self.concave_list))

        self.cutline_count=0
        #print(len(self.approxs))
        self.pointcutlist=[[] for i in range(len(self.point_map))]
        while(len(self.concave_list)>0):
            vp=self.concave_list.pop()
            self.ViewablePoint(vp)
            self.WeightCut(vp)

            self.pointcutlist[self.cutline[0]].append(self.cut_line([self.cutline[1],self.cutline_count]))
            self.pointcutlist[self.cutline[1]].append(self.cut_line([self.cutline[0],self.cutline_count]))
            self.cutlinelist.append(self.cutline)
            self.cutline_count+=1

            if self.newconcave.key != 0:
                self.concave_list.append(self.newconcave)
            for concave in self.concave_list:
                if concave.N == self.cutline[1]:
                    v3=[self.point_map[vp.N][0] - self.point_map[concave.N][0],
                        self.point_map[vp.N][1] - self.point_map[concave.N][1]]
                    v1=[self.point_map[concave.N][0] - self.point_map[concave.S][0],
                        self.point_map[concave.N][1] - self.point_map[concave.S][1]]
                    r = np.cross(v1, v3)

                    temp_newconcave=self.VP([0,0,0,0])
                    if r < 0:
                        temp_newconcave = self.VP([concave.S, concave.N, vp.N, -1])
                    else:
                        v1=[self.point_map[concave.E][0] - self.point_map[concave.N][0],
                            self.point_map[concave.E][1] - self.point_map[concave.N][1]]
                        r = np.cross(v1, v3)
                        if r < 0:
                            temp_newconcave = self.VP([vp.N,concave.N, concave.E,  -1])
                    self.concave_list.remove(concave)
                    if temp_newconcave.key != 0:
                        self.concave_list.append(temp_newconcave)
                    break

    def WeightCut(self,vp):
        #PointMap = self.point_map
        # vp = self.VP(concave_list)
        #vplist=self.vplist

        temp_vcx, temp_vcy = self.point_map[self.vplist[0]][0]- self.point_map[vp.N][0], self.point_map[self.vplist[0]][1]- self.point_map[vp.N][1]
        #temp_vcx, temp_vcy = PointMap[vp.N][0] - PointMap[vplist[0]][0], PointMap[vp.N][1] - PointMap[vplist[0]][1]
        vc=[temp_vcx,temp_vcy]
        temp_s = np.linalg.norm(vc)
        vc_unit=vc/temp_s

        v1 = [self.point_map[vp.N][0]-self.point_map[vp.S][0],self.point_map[vp.N][1]-self.point_map[vp.S][1]]
        v2 = [self.point_map[vp.N][0]-self.point_map[vp.E][0],self.point_map[vp.N][1]-self.point_map[vp.E][1]]
        v1_unit = v1 / np.linalg.norm(v1)  # 将向量v1归一化为单位向量
        v2_unit = v2 / np.linalg.norm(v2)  # 将向量v2归一化为单位向量
        bisector = v1_unit + v2_unit
        bisector = bisector / np.linalg.norm(bisector)  # 将角平分线向量归一化为单位向量

        if np.dot( bisector,vc_unit)>0:
            temp_min =  (abs(np.cross(bisector, vc_unit))-1)/temp_s
            #temp_min = (abs(np.cross(bisector, vc_unit)) - 1)
        else:
            temp_min = (abs(np.dot(bisector, vc_unit)) )*temp_s
            #temp_min = (abs(np.dot(bisector, vc_unit)))

        p_min = self.vplist[0]  # 可以把这个改成self.p_min
        # self.p_min = self.vplist[0]#可以把这个改成self.p_min
        for i in range(1, len(self.vplist)):
            p = self.vplist[i]

            #temp_vcx, temp_vcy = PointMap[vp.N][0] - PointMap[p][0], PointMap[vp.N][1] - PointMap[p][1]
            temp_vcx, temp_vcy = self.point_map[p][0] - self.point_map[vp.N][0], self.point_map[p][1] - self.point_map[vp.N][1]
            vc = [temp_vcx, temp_vcy]
            temp_s = np.linalg.norm(vc)
            vc_unit = vc / temp_s

            if np.dot(bisector, vc_unit) > 0:
                temp =  (abs(np.cross(bisector, vc_unit))-1)/temp_s
                #temp = (abs(np.cross(bisector, vc_unit)) - 1)
            else:
                temp = (abs(np.dot(bisector, vc_unit)))*temp_s
                #temp = (abs(np.dot(bisector, vc_unit)))
            #temp = temp_s * abs(np.cross( bisector,vc_unit))

            if temp < temp_min:
                temp_min = temp
                # self.p_min = p # 把这个和上面和下面那个p_min改成self.p_min可以验证一下
                p_min = p
        # p_min=self.p_min #这是下面这个


        self.cutline = [vp.N, p_min]  # 凸剖分的切割线

        v3 = [self.point_map[p_min][0] - self.point_map[vp.N][0], self.point_map[p_min][1] - self.point_map[vp.N][1]]
        v1 = [self.point_map[vp.N][0] - self.point_map[vp.S][0], self.point_map[vp.N][1] - self.point_map[vp.S][1]]
        r = np.cross(v1, v3)
        if r < 0:
            self.newconcave=self.VP([vp.S, vp.N, p_min, -1])
            return

        v1 = [self.point_map[vp.E][0] - self.point_map[vp.N][0], self.point_map[vp.E][1] - self.point_map[vp.N][1]]
        r = np.cross(v1, v3)
        if r < 0:
            self.newconcave=self.VP([p_min, vp.N, vp.E, -1])
            return
        self.newconcave=self.VP([0, 0, 0, 0])

    class VP:
        def __init__(self,concave):
            self.S = concave[0]
            self.N = concave[1]
            self.E = concave[2]
            self.key = concave[3]

    class Line:
        def __init__(self,linelist):
            self.S = linelist[0]
            self.E = linelist[1]

    def ViewablePoint(self,vp):
        ViewableSign = 0
        vplistlen = 0
        PointMaplen=len(self.point_map)
        #PointMap=self.point_map
        #vp=self.VP(concave_list)#测试用
        linelistlen=len(self.linelist)
        self.vplist=[]
        # line = self.Line(self.linelist[10])
        # print(line.E )


        for i in range(0,PointMaplen):  # Loop over all visible points
            ViewableSign = 0
            if i != vp.S and i != vp.E and i != vp.N and self.point_map[i][0] >= 0:
                #判断是否在连通域里
                v1 = [self.point_map[vp.N][0] - self.point_map[vp.S][0],
                      self.point_map[vp.N][1] - self.point_map[vp.S][1]]
                v2 = [self.point_map[vp.E][0] - self.point_map[vp.N][0],
                      self.point_map[vp.E][1] - self.point_map[vp.N][1]]
                v3 = [self.point_map[i][0] - self.point_map[vp.N][0],
                      self.point_map[i][1] - self.point_map[vp.N][1]]
                v1p = np.cross(v1, v3)
                v2p = np.cross(v2, v3)

                # v1_x = PointMap[vp.N][0] - PointMap[vp.S][0]
                # v1_y = PointMap[vp.N][1] - PointMap[vp.S][1]
                # v2_x = PointMap[vp.E][0] - PointMap[vp.N][0]
                # v2_y = PointMap[vp.E][1] - PointMap[vp.N][1]
                # v3_x = PointMap[i][0] - PointMap[vp.N][0]
                # v3_y = PointMap[i][1] - PointMap[vp.N][1]
                # v1p = v1_x * v3_y - v1_y * v3_x
                # v2p = v2_x * v3_y - v2_y * v3_x
                if (vp.key > 0 and v1p > 0 and v2p > 0) or (vp.key < 0 and (v1p > 0 or v2p > 0)):
                # 是凸点，且切割点在连通域里||是凹点，且切割点是在连通域里的
                    ViewableSign = 1
                    for line_num in range(0,linelistlen):
                        line = self.linelist[line_num]
                        if vp.N == line.S or vp.N == line.E or i == line.S or i == line.E:
                            continue
                        v1 = [self.point_map[line.S][0] - self.point_map[vp.N][0],
                              self.point_map[line.S][1] - self.point_map[vp.N][1]]
                        v2 = [self.point_map[i][0] - self.point_map[vp.N][0],
                              self.point_map[i][1] - self.point_map[vp.N][1]]
                        v3 = [self.point_map[line.E][0] - self.point_map[vp.N][0],
                              self.point_map[line.E][1] - self.point_map[vp.N][1]]
                        tr1 = np.cross(v1, v2)
                        tr2 = np.cross(v3, v2)
                        r1=tr1*tr2

                        # v1_x = PointMap[line.S][0] - PointMap[vp.N][0]
                        # v1_y = PointMap[line.S][1] - PointMap[vp.N][1]
                        # v2_x = PointMap[i][0] - PointMap[vp.N][0]
                        # v2_y = PointMap[i][1] - PointMap[vp.N][1]
                        # v3_x = PointMap[line.E][0] - PointMap[vp.N][0]
                        # v3_y = PointMap[line.E][1] - PointMap[vp.N][1]
                        # r1 = (v1_x * v2_y - v1_y * v2_x) * (v3_x * v2_y - v3_y * v2_x)

                        v1 = [self.point_map[i][0] - self.point_map[line.S][0],
                              self.point_map[i][1] - self.point_map[line.S][1]]
                        v2 = [self.point_map[line.E][0] - self.point_map[line.S][0],
                              self.point_map[line.E][1] - self.point_map[line.S][1]]
                        v3 = [self.point_map[vp.N][0] - self.point_map[line.S][0],
                              self.point_map[vp.N][1] - self.point_map[line.S][1]]
                        tr1 = np.cross(v1, v2)
                        tr2 = np.cross(v3, v2)
                        r2=tr1*tr2
                        # v1_x = PointMap[i][0] - PointMap[line.S][0]
                        # v1_y = PointMap[i][1] - PointMap[line.S][1]
                        # v2_x = PointMap[line.E][0] - PointMap[line.S][0]
                        # v2_y = PointMap[line.E][1] - PointMap[line.S][1]
                        # v3_x = PointMap[vp.N][0] - PointMap[line.S][0]
                        # v3_y = PointMap[vp.N][1] - PointMap[line.S][1]
                        # r2 = (v1_x * v2_y - v1_y * v2_x) * (v3_x * v2_y - v3_y * v2_x)

                        if r1 <= 0 and r2 <= 0:
                            if r1 == 0 and r2 == 0:
                                d1 = (self.point_map[i][0] - self.point_map[line.S][0]) * (self.point_map[i][0] - self.point_map[line.E][0]) + (
                                            self.point_map[i][1] - self.point_map[line.S][1]) * (self.point_map[i][1] - self.point_map[line.E][1])
                                d2 = (self.point_map[vp.N][0] - self.point_map[line.S][0]) * (self.point_map[vp.N][0] - self.point_map[line.E][0]) + (
                                            self.point_map[vp.N][1] - self.point_map[line.S][1]) * (self.point_map[vp.N][1] - self.point_map[line.E][1])
                                if d1 < 0 or d2 < 0:
                                    ViewableSign = 0
                                    break
                            else:
                                ViewableSign = 0
                                break

                    for line_num in range(0,len(self.cutlinelist)):
                        line = self.Line(self.cutlinelist[line_num])
                        if vp.N == line.S or vp.N == line.E or i == line.S or i == line.E:
                            continue
                        v1 = [self.point_map[line.S][0] - self.point_map[vp.N][0],
                              self.point_map[line.S][1] - self.point_map[vp.N][1]]
                        v2 = [self.point_map[i][0] - self.point_map[vp.N][0],
                              self.point_map[i][1] - self.point_map[vp.N][1]]
                        v3 = [self.point_map[line.E][0] - self.point_map[vp.N][0],
                              self.point_map[line.E][1] - self.point_map[vp.N][1]]
                        tr1 = np.cross(v1, v2)
                        tr2 = np.cross(v3, v2)
                        r1=tr1*tr2

                        # v1_x = PointMap[line.S][0] - PointMap[vp.N][0]
                        # v1_y = PointMap[line.S][1] - PointMap[vp.N][1]
                        # v2_x = PointMap[i][0] - PointMap[vp.N][0]
                        # v2_y = PointMap[i][1] - PointMap[vp.N][1]
                        # v3_x = PointMap[line.E][0] - PointMap[vp.N][0]
                        # v3_y = PointMap[line.E][1] - PointMap[vp.N][1]
                        # r1 = (v1_x * v2_y - v1_y * v2_x) * (v3_x * v2_y - v3_y * v2_x)

                        v1 = [self.point_map[i][0] - self.point_map[line.S][0],
                              self.point_map[i][1] - self.point_map[line.S][1]]
                        v2 = [self.point_map[line.E][0] - self.point_map[line.S][0],
                              self.point_map[line.E][1] - self.point_map[line.S][1]]
                        v3 = [self.point_map[vp.N][0] - self.point_map[line.S][0],
                              self.point_map[vp.N][1] - self.point_map[line.S][1]]
                        tr1 = np.cross(v1, v2)
                        tr2 = np.cross(v3, v2)
                        r2=tr1*tr2
                        # v1_x = PointMap[i][0] - PointMap[line.S][0]
                        # v1_y = PointMap[i][1] - PointMap[line.S][1]
                        # v2_x = PointMap[line.E][0] - PointMap[line.S][0]
                        # v2_y = PointMap[line.E][1] - PointMap[line.S][1]
                        # v3_x = PointMap[vp.N][0] - PointMap[line.S][0]
                        # v3_y = PointMap[vp.N][1] - PointMap[line.S][1]
                        # r2 = (v1_x * v2_y - v1_y * v2_x) * (v3_x * v2_y - v3_y * v2_x)

                        if r1 <= 0 and r2 <= 0:
                            if r1 == 0 and r2 == 0:
                                d1 = (self.point_map[i][0] - self.point_map[line.S][0]) * (self.point_map[i][0] - self.point_map[line.E][0]) + (
                                            self.point_map[i][1] - self.point_map[line.S][1]) * (self.point_map[i][1] - self.point_map[line.E][1])
                                d2 = (self.point_map[vp.N][0] - self.point_map[line.S][0]) * (self.point_map[vp.N][0] - self.point_map[line.E][0]) + (
                                            self.point_map[vp.N][1] - self.point_map[line.S][1]) * (self.point_map[vp.N][1] - self.point_map[line.E][1])
                                if d1 < 0 or d2 < 0:
                                    ViewableSign = 0
                                    break
                            else:
                                ViewableSign = 0
                                break

                    if( ViewableSign == 1):
                        self.vplist.append(i)

    class island:
        def __init__(self):
            self.point = []
            self.cutline = []
            xmin,xmax,ymin,ymax = float('inf'),-1,float('inf'),-1
            self.boundary=[xmin,xmax,ymin,ymax]

    class cut_line:
        def __init__(self,cutline):
            self.plink= cutline[0]#连接点
            self.cutline = cutline[1]#切线编号

    class bridges:
        def __init__(self):
            self.bridge2land= []#桥连接的岛
            self.bridge2bridge = []#桥可去的其他桥桥

    def GetPloygon(self):
        self.bridgelist = [self.bridges() for i in range(len(self.cutlinelist))]
        if len(self.cutlinelist) == 0:
            self.islandlist = [self.island() for i in range(1)]
            for i in range(len(self.point_map)):
                self.islandlist[0].point.append(i)
            self.islandlist[0].cutline = []
            return
        for pointcut_count in range(len(self.point_map)):
            pointcut = self.pointcutlist[pointcut_count]
            if len(pointcut) == 0:
                continue
            for cutline_count in pointcut:
                island_temp = self.island()
                cutline = cutline_count
                island_temp.point.append(pointcut_count)
                island_temp.cutline.append(cutline.cutline)
                self.bridgelist[cutline.cutline].bridge2land.append(len(self.islandlist))

                Pago = pointcut_count
                Pnow = cutline.plink
                while Pnow != pointcut_count:
                    island_temp.point.append(Pnow)
                    if len(self.pointcutlist[Pnow]) == 0:
                        Pago = Pnow
                        Pnow = self.point_list[Pnow].E
                    else:
                        Pnex, pointlist_nex = self.point_list[Pnow].E, self.point_list[Pnow].E
                        nnex = self.point_map[Pnex]
                        nnow = self.point_map[Pnow]
                        nago = self.point_map[Pago]
                        v1 = [nnow[0] - nago[0],nnow[1] - nago[1]]
                        v2 = [nnex[0] - nnow[0],nnex[1] - nnow[1]]
                        if np.cross(v1, v2) >= 0:
                            minangle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
                        else:
                            minangle = 1.20
                        minpoint_count = None
                        for Pnex_count in self.pointcutlist[Pnow]:
                            Pnex = Pnex_count.plink
                            if Pnex != Pago:
                                nnex = self.point_map[Pnex]
                                nnow = self.point_map[Pnow]
                                nago = self.point_map[Pago]
                                v1 = [nnow[0] - nago[0], nnow[1] - nago[1]]
                                v2 = [nnex[0] - nnow[0], nnex[1] - nnow[1]]
                                if np.cross(v1, v2) >= 0:
                                    cosangle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2))
                                    if cosangle < minangle:
                                        minpoint_count = Pnex_count
                                        minangle = cosangle
                        if minpoint_count is not None:
                            island_temp.cutline.append(minpoint_count.cutline)
                            self.bridgelist[minpoint_count.cutline].bridge2land.append(len(self.islandlist))
                            Pago = Pnow
                            Pnow = minpoint_count.plink
                            self.pointcutlist[Pago].remove(minpoint_count)
                        else:
                            Pago=Pnow
                            Pnow=pointlist_nex
                self.islandlist.append(island_temp)
        for i in range(len(self.bridgelist)):#遍历桥（桥对应连接的岛）
            for land in self.bridgelist[i].bridge2land:#遍历连接的岛
                for bridges in self.islandlist[land].cutline:
                    if i != bridges:
                        self.bridgelist[i].bridge2bridge.append(bridges)
        # 判断边界
        for i in range(len(self.islandlist)):
            land =self.islandlist[i]
            xmin, xmax, ymin, ymax = float('inf'), -1, float('inf'), -1
            for point in land.point:
                if self.point_map[point][0] > xmax:
                    xmax = self.point_map[point][0]
                if self.point_map[point][0] < xmin:
                    xmin = self.point_map[point][0]
                if self.point_map[point][1] > ymax:
                    ymax = self.point_map[point][1]
                if self.point_map[point][1] < ymin:
                    ymin = self.point_map[point][1]
            self.islandlist[i].boundary = [xmin, xmax, ymin, ymax]

    def DrawBImap(self, debugmap,IMGmap):
        self.IMGmap=IMGmap
        for landnum in range(len(self.islandlist)):
            land_node = []
            land=self.islandlist[landnum]
            for land_point in land.point:
                land_node.append(self.point_map[land_point])
            if debugmap:
                color = tuple(np.random.randint(0, 255, 3).tolist())
            else:
                color=(landnum,255,255)
            points_array = np.array(land_node)
            cv2.fillConvexPoly(self.IMGmap, points_array, color)

if __name__ == "__main__":

    path = '/home/sy/amrrt/demos/resources/maze.png'
    ratio = 0.02
    # ratio = 0.015
    robotsize = 0.6
    thresh = 128

    IMGmap = cv2.imread(path)

    # #调整图像大小
    image_size = 800
    height, width, channels = IMGmap.shape
    scale = image_size/min(height,width)#比例，通用的大小为500
    new_height,new_width = int(height*scale), int(width*scale)#长宽中至少有一个是500
    IMGmap = cv2.resize(IMGmap, (new_width, new_height))  # 调整图形大小

    # cv2.imshow("1", IMGmap)
    Approx=approx()
    returndata = Approx.approx_work(IMGmap, ratio, robotsize, thresh)
    #print(Approx.approxs)
    # 输入处理过后的路径，比例，机器人大小，
    for i in range(0, len(returndata[0])):  # 遍历连通域,大多数情况下只有一个连通域,即len(returndata[0]）是1
        Approx.find_concave(returndata[0][i])
        #print(returndata[0][i])
        Approx.Startcut()
        Approx.GetPloygon()
        #Approx.DrawBImap(False,IMGmap)



    #print(Approx.concave_list)
    #print(Approx.point_map)

    ###以下是验证代码是否正确的###
    # temp=IMGmap


    # # 创建纯白色图像
    height, width, channels = IMGmap.shape
    temp = np.zeros((height, width, 3), np.uint8)
    temp.fill(255)
    width, height = 10, 10

    cv2.drawContours(temp, Approx.approxs, -1, (0, 0, 0), 2)#画拟合线

    # first=True
    # for i in range(0, len(Approx.returnlist)):#画拟合的顶点
    #     #color = (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255))
    #     #print(returnlist[i])
    #     for point in Approx.returnlist[i] :
    #          #print(point)
    #          if point[0]!=-1 and first:
    #             cv2.circle(temp, point, 10, (0, 0, 0), -1)
    #          elif point[0]!=-1 and first==False:
    #              width, height = 10, 10
    #              # 画实心方形点
    #              cv2.rectangle(temp, (point[0]- width, point[1]-height), (point[0]+ width, point[1] + height), (0, 0, 0), -1)
    #          else:
    #             color = (random.randint(0, 255), random.randint(0, 255), random.randint(0, 255))
    #             first = False


    # #print(Approx.point_map[Approx.concave_list[10][1]])
    # for concave_point in Approx.concave_list:#画凹点，要凸剖分的
    #     cv2.circle(temp, Approx.point_map[concave_point.N], 10, (0,0,0), -1)

    #画单个凹点记得把
    #Approx.GetPloygon()
    #Approx.DrawBImap(False,IMGmap)
    #注释掉

    # point=Approx.point_map[Approx.concave_list[30].N]
    # cv2.rectangle(temp, (point[0] - width, point[1] - height), (point[0] + width, point[1] + height), (0, 0, 0), -1)
    # #选一个凸点,另一个在startcut里改
    # for vp_point in Approx.vplist:#画可视点
    #     cv2.circle(temp, Approx.point_map[vp_point], 10, (0,0,0), -1)
    #
    # #画最优可视点 #记得把p_min改一下（改三处）
    # cv2.circle(temp, Approx.point_map[Approx.p_min], 5, (255,255,255), -1)
    #
    # #画剖分后的新凸点
    # if Approx.newconcave.key!=0:
    #     cv2.circle(temp, Approx.point_map[Approx.newconcave], 5, (255,255,0), 2)
    # #画切线
    # cv2.line(temp, Approx.point_map[Approx.cutline[0]], Approx.point_map[Approx.cutline[1]], (0, 0, 0), 3)


    # #画剖分后的切线
    for cut_line in Approx.cutlinelist :
        cv2.line(temp, Approx.point_map[cut_line[0]],Approx.point_map[cut_line[1]],  (0,0,0),1)


    # 画剖分后的单个凸多边形，
    # bridgelist[切割线]=[切割线的一个岛1，切割线的一个岛2]
    # 岛群，一个岛代表一个凸多边形，由点和凸剖分的切割线组成
    # testcut=30
    # lands=Approx.bridgelist[testcut].bridge2land#通过桥来遍历
    # #land = Approx.islandlist[10]
    # for land_num in lands:#遍历桥两侧的岛
    #     land=Approx.islandlist[land_num]
    #     for land_point in land.point:#画岛的点
    #         cv2.circle(temp, Approx.point_map[land_point], 5, (255, 255, 0), 2)
    #     for land_cutline in land.cutline:#画岛的切割线
    #         start,end=Approx.cutlinelist[land_cutline][0],Approx.cutlinelist[land_cutline][1]
    #         cv2.line(temp, Approx.point_map[start],Approx.point_map[end],  (0,0,0),1)
    # cv2.line(temp, Approx.point_map[Approx.cutlinelist[testcut][0]], Approx.point_map[Approx.cutlinelist[testcut][1]],
    #          (255, 0, 0), 1)#中心切割线另外画

    # testcut=33
    # bridges = Approx.bridgelist[testcut].bridge2bridge  # 通过桥来遍历
    # #land = Approx.islandlist[10]
    # for bridge in bridges:#遍历桥能去的桥
    #     cv2.line(temp, Approx.point_map[Approx.cutlinelist[bridge][0]],Approx.point_map[Approx.cutlinelist[bridge][1]],  (0,0,0),1)
    # cv2.line(temp, Approx.point_map[Approx.cutlinelist[testcut][0]], Approx.point_map[Approx.cutlinelist[testcut][1]],
    #          (255, 0, 0), 1)#中心切割线另外画

    cv2.imshow('approxs', temp)
    cv2.waitKey()